package gridmapper.server.planner;
import gridmapper.RobotHeading;

import java.awt.Point;
import java.util.ArrayList;

import org.jgrapht.alg.*;
import org.jgrapht.graph.DefaultEdge;
import org.jgrapht.graph.SimpleGraph;


public class GridGrapher {
	 
	public static void main(String[] args)
	{
		Point src = new Point(0,1);
		Point tgt = new Point(3,0);

		GridPlanner roomba = new GridPlanner(4,6, new Point(0,0), RobotHeading.NORTH);
		roomba.run();

		GridGrapher grapher = new GridGrapher(roomba, src, tgt);
		
		grapher.run();
		for (Point move : grapher.moves)
		{
			System.out.println(move.x + "," + move.y);
		}

	}
	
	public ArrayList<Point> moves;
	public ArrayList<DefaultEdge> edges;
	SimpleGraph<Point, DefaultEdge> graph;

	private GridPlanner roomba;
	private Point startPosition;
	private Point endPosition;
	
	public GridGrapher(GridPlanner roombaIn, Point start, Point end)
	{
		roomba = roombaIn;
		startPosition = start;
		endPosition = end;
		init();
	}
	public void init()
	{
		graph = new SimpleGraph<Point, DefaultEdge>(DefaultEdge.class);
		if (roomba == null || startPosition == null || endPosition == null) //if we haven't set any values: abort
			return;
		
		for (int x = 0; x < roomba.gridX; x++) //adds all of the cells as a vertex
		{
			for (int y = 0; y < roomba.gridY; y++)
			{
				graph.addVertex(new Point(x,y));
			}
		}
		
		for (int x = 0; x < roomba.gridX; x++) //adds edges if it is possible to use the adjacent cell
		{
			for (int y = 0; y < roomba.gridY; y++)
			{
				if (roomba.cells[y][x] != CellState.scannedOccupied) //if the current cell isn't occupied
				{
					if (roomba.isCellValid(x-1, y)) //east cell
					{
						graph.addEdge(new Point(x,y), new Point(x-1,y));
					}
					if (roomba.isCellValid(x+1, y)) //west cell
					{
						graph.addEdge(new Point(x,y), new Point(x+1,y));
					}
					if (roomba.isCellValid(x, y-1)) //north cell
					{
						graph.addEdge(new Point(x,y), new Point(x,y-1));
					}
					if (roomba.isCellValid(x, y+1)) //south cell
					{
						graph.addEdge(new Point(x,y), new Point(x,y+1));
					}
				}
			}
		}
	}
	public void run()
	{
		DijkstraShortestPath<Point, DefaultEdge> path 
			= new DijkstraShortestPath<Point, DefaultEdge>(graph, startPosition, endPosition, Double.POSITIVE_INFINITY);
		
		edges = (ArrayList<DefaultEdge>) path.getPathEdgeList(); //gets the list of edges created from this path
		if (edges == null)
			return;
		
		moves = new ArrayList<Point>();
		Point oldSrc = null;
		Point oldTgt = null;
		
		for (DefaultEdge edge : edges)
		{
			Point startPoint = getPointFromDefaultEdge(edge.toString(), 0);
			Point endPoint = getPointFromDefaultEdge(edge.toString(), 1);
			
			
			//keeps the list in proper order.. I.E line one: X -> Y  line two: Y -> Z
			//rather than X->Y, Z->Y.. both are technically correct since the graph is undirected, 
			//but we want to use the 2nd point to gather a list of moves for the robot
			//**if the old source position is = to the new source position. swap the new source with new target
			//**ditto with old target position
			if ((oldSrc != null && startPoint.x == oldSrc.x && startPoint.y == oldSrc.y) 
					|| (oldTgt != null && endPoint.x == oldTgt.x && endPoint.y == oldTgt.y))  
			{
				Point temp = (Point) startPoint.clone();
				startPoint = endPoint;
				endPoint = temp;
				
			}
			//this will then be our first move
			//this ensures that our first start point = start location
			else if (oldSrc == null) 
			{
				if (startPosition.x != startPoint.x || startPosition.y != startPoint.y) 
				{
					Point temp = (Point) startPoint.clone();
					startPoint = endPoint;
					endPoint = temp;
				}
			}
			oldSrc = startPoint; //set our old source and target for the next iteration
			oldTgt = endPoint;
			
			System.out.println(startPoint + " : " + endPoint); //debug statement
			
			moves.add(endPoint); //at the target point to the list of moves
		}
		System.out.println("=======================================");
		
		
	}
	public static Point getPointFromDefaultEdge(String input, int position)
	{
		//replace all of the unneeded characters
		//input is in form (java.awt.Point[x=0,y=0] : java.awt.Point[x=1,y=0])
		String[] split1 = input.replace("]", "").replace("[", "").replace("(", "").replace(")", "").replace("x=", "").replace("y=", "").replace("java.awt.Point", "").split(":"); 
		
		//split the two halves into two separate points 
		String[] split2 = split1[position].trim().split(",");
		
		//return the point
		return new Point(Integer.parseInt(split2[0]), Integer.parseInt(split2[1]));
		
		
	}
	
}
